'''
Created on Mar 29, 2010

@author: jhlee
'''
import numpy as np

class Matrix44:
    def __init__(self):
        self.identity()
        
    def copyFrom(self, rhs):
        self.n11 = rhs.n11
        self.n12 = rhs.n12
        self.n13 = rhs.n13
        self.n14 = rhs.n14
        
        self.n21 = rhs.n21
        self.n22 = rhs.n22
        self.n23 = rhs.n23
        self.n24 = rhs.n24
        
        self.n31 = rhs.n31
        self.n32 = rhs.n32
        self.n33 = rhs.n33
        self.n34 = rhs.n34
        
        self.n41 = rhs.n41
        self.n42 = rhs.n42
        self.n43 = rhs.n43
        self.n44 = rhs.n44
        
        
    def identity(self):
        self.n11 = 1
        self.n12 = 0
        self.n13 = 0
        self.n14 = 0
        
        self.n21 = 0
        self.n22 = 1
        self.n23 = 0
        self.n24 = 0
        
        self.n31 = 0
        self.n32 = 0
        self.n33 = 1
        self.n34 = 0
        
        self.n41 = 0
        self.n42 = 0
        self.n43 = 0
        self.n44 = 1
        
    def multiply(self, a, b):
        a11 = a.n11
        a12 = a.n12
        a13 = a.n13
        a14 = a.n14
        
        a21 = a.n21
        a22 = a.n22
        a23 = a.n23
        a24 = a.n24
        
        a31 = a.n31
        a32 = a.n32
        a33 = a.n33
        a34 = a.n34
        
        a41 = a.n41
        a42 = a.n42
        a43 = a.n43
        a44 = a.n44
        
        b11 = b.n11
        b12 = b.n12
        b13 = b.n13
        b14 = b.n14
        
        b21 = b.n21
        b22 = b.n22
        b23 = b.n23
        b24 = b.n24
        
        b31 = b.n31
        b32 = b.n32
        b33 = b.n33
        b34 = b.n34
        
        b41 = b.n41
        b42 = b.n42
        b43 = b.n43
        b44 = b.n44
        
        self.n11 = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41
        self.n12 = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42
        self.n13 = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43
        self.n14 = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44
        
        self.n21 = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41
        self.n22 = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42
        self.n23 = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43
        self.n24 = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44
        
        self.n31 = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41
        self.n32 = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42
        self.n33 = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43
        self.n34 = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44
        
        self.n41 = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41
        self.n42 = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42
        self.n43 = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43
        self.n44 = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44
        
    def makeTransform(self, position, scale, quatOrientation):
        '''
        Matrix build up order:
        1. Scale
        2. Rotate
        3. Translate
        '''
        self.identity()
        
        matRot = Matrix44()
        quatOrientation.toRotationMatrix(matRot)
        
        matScale = Matrix44()
        matScale.n11 = scale.x
        matScale.n22 = scale.y
        matScale.n33 = scale.z
        
        matRot.multiply(matRot, matScale)        
        self.copyFrom(matRot)
        self.setTrans(position)
        
        self.n41 = 0
        self.n42 = 0
        self.n43 = 0
        self.n44 = 1
        
    def setTrans(self, pos):
        self.n14 = pos.x
        self.n24 = pos.y
        self.n34 = pos.z
        

    def toTuple(self):
        return (self.n11,self.n12,self.n13,self.n14,
                self.n21,self.n22,self.n23,self.n24,
                self.n31,self.n32,self.n33,self.n34,
                self.n41,self.n42,self.n43,self.n44)
        
    def debugDump(self):
        print '%6d%6d%6d%6d\n%6d%6d%6d%6d\n%6d%6d%6d%6d\n%6d%6d%6d%6d' % self.toTuple()
        
    def toArray(self):
        xform = np.zeros([4,4])
        xform[0,0] = self.n11
        xform[0,1] = self.n12
        xform[0,2] = self.n13
        xform[0,3] = self.n14
        
        xform[1,0] = self.n21
        xform[1,1] = self.n22
        xform[1,2] = self.n23
        xform[1,3] = self.n24
        
        xform[2,0] = self.n31
        xform[2,1] = self.n32
        xform[2,2] = self.n33
        xform[2,3] = self.n34
        
        xform[3,0] = self.n41
        xform[3,1] = self.n42
        xform[3,2] = self.n43
        xform[3,3] = self.n44
        
        #return xform.transpose()
        return xform.transpose()
    